import launch
from launch.substitutions import Command, LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from nav2_common.launch import RewrittenYaml
import launch_ros
from launch.actions import TimerAction
import os

def generate_launch_description():
    pkg_share = launch_ros.substitutions.FindPackageShare(package='nmsl_bringup').find('nmsl_bringup')
   
    world_path=os.path.join(pkg_share, 'world/RMUC.sdf')

    default_model_path = os.path.join(pkg_share, 'src/description/real.urdf')

    # 机器人初始位置
    spawn_entity = launch_ros.actions.Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'sam_bot', '-topic', 'robot_description','-x','-2.5','-y','-10.0','-z','0.5'],#7v7
        # arguments=['-entity', 'sam_bot', '-topic', 'robot_description','-x','7','-y','5','-z','1.0'],#7v7
        parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
        output='screen'   
    )

    robot_state_publisher_node = launch_ros.actions.Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])},
                    {'use_sim_time': LaunchConfiguration('use_sim_time')}]
    )
    
    joint_state_publisher_node = launch_ros.actions.Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
    )

    global_planner_cmd=launch_ros.actions.Node(
            package='global_planner',
            executable='global_planner',
            name='global_planner',
            parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')},
                        {'map_path': LaunchConfiguration('map_path')}],
            output='screen'
    )

    apf_planner_cmd=launch_ros.actions.Node(
            package='local_planner',
            executable='ApfPlanner',
            name='ApfPlanner',
            parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
            output='screen',
    )

    slam_cmd=launch_ros.actions.Node(
            package='slam_gmapping', 
            executable='slam_gmapping', 
            output='log',)

    point_clout_convert =launch_ros.actions.Node(
            package="pointcloud_to_laserscan",
            executable="pointcloud_to_laserscan_node",
            remappings=[('/cloud_in','/segmentation/obstacle')],
            parameters=[{"min_height":0.0},
                        {"max_height":0.6},
                        {"scan_time":0.1},
                        {"range_min":0.3},
                        {"range_max":10.0},
                        {"angle_increment":3.14159/900},
                        {"target_frame":"base_link"}] # 0.2deg
            )

    map_tf2_publish_cmd = launch_ros.actions.Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            parameters=[{'use_sim_time': LaunchConfiguration('use_sim_time')}],
            arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
            output='screen'
        )
    rviz_node = launch_ros.actions.Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            output='screen',
            arguments=['-d', os.path.join(pkg_share, 'rviz', '111.rviz')],
        )
    

    return launch.LaunchDescription([
        launch.actions.DeclareLaunchArgument(name='map_path', default_value='map_5v5.pgm',
                                            description='Absolute path to map file'),
        launch.actions.DeclareLaunchArgument(name='world', default_value=world_path,
                                            description='Absolute path to world config file'),
        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
                                            description='Absolute path to robot urdf file'),
        launch.actions.DeclareLaunchArgument(name='use_sim_time', default_value='true',
                                            description='Flag to enable use_sim_time'),
        launch.actions.ExecuteProcess(cmd=['gazebo', '--verbose', '-s', 'libgazebo_ros_init.so', 
                                           '-s', 'libgazebo_ros_factory.so',
                                           LaunchConfiguration('world')], 
                                           output='screen'),

        spawn_entity,
        TimerAction(period=8.0, actions=[rviz_node]),
        robot_state_publisher_node,
        joint_state_publisher_node,
        map_tf2_publish_cmd,
        global_planner_cmd,
        apf_planner_cmd
        
    ])
